#pragma config(Sensor, in1,    Light,          sensorReflection)
#pragma config(Sensor, in2,    RightLine,      sensorLineFollower)
#pragma config(Sensor, in3,    MiddleLine,     sensorLineFollower)
#pragma config(Sensor, in4,    LeftLine,       sensorLineFollower)
#pragma config(Sensor, dgtl1,  Bumper,         sensorTouch)
#pragma config(Motor,  port2,           LeftMotor,     tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           RightMotor,    tmotorVex269, openLoop)
#pragma config(Motor,  port6,           LeftTop,       tmotorVex269, openLoop)
#pragma config(Motor,  port8,           ClawMotor,     tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#define STOPPED 0
#define FORWARD 1
#define TURNLEFT 2
#define TURNRIGHT 3

int checkLight() {
  int lightsensed = 0;
  int lightvalue = SensorValue (Light);
  if (lightvalue <100) {
    lightsensed = 1;
  }
  return lightsensed;
}

int checklineright() {
  int linesensed=0;
  int linevalue=SensorValue (RightLine);
  if (linevalue<1500){
    linesensed=1;
  }
  return linesensed;
}

int checklinemiddle() {
  int linesensed=0;
  int linevalue=SensorValue (MiddleLine);
  if (linevalue<1500){
    linesensed=1;
  }
  return linesensed;
}

int checklineleft() {
  int linesensed=0;
  int linevalue=SensorValue (LeftLine);
  if (linevalue<1000){
    linesensed=1;
  }
  return linesensed;
}

bool checkBump(){
  return SensorValue (Bumper);
}

void moveRobot (int speed){
  motor[LeftMotor]=speed;
  motor[RightMotor]=speed;
}

void turnRobot (int leftspeed,int rightspeed){
  motor [LeftMotor]=leftspeed;
  motor [RightMotor]=rightspeed;
}
task main (){
  int robotstate =STOPPED;
  while (true){
    int lightsensed = checkLight();
    bool bumpsensed = checkBump ();
    int linesensedr = checklineright();
    int linesensedm = checklinemiddle();
    int linesensedl = checklineleft();
    switch (robotstate){
    case STOPPED:
      if (lightsensed){
        moveRobot(127);
        robotstate=FORWARD;
        break;
      }
      if (linesensedm){
        moveRobot (127);
        robotstate=FORWARD;
        break;
      }

    case FORWARD:
      if (bumpsensed) {
        moveRobot(0);
        robotstate = STOPPED;
        break;
}
        if (linesensedl){
          turnRobot (127,-127);
          robotstate=TURNRIGHT;
          break;
  }
        if (linesensedr){
          turnRobot (-127,127);
          robotstate=TURNLEFT;
                }

     case TURNRIGHT:
         if  (linesensedm){
         moveRobot (127);
         robotstate= FORWARD;
         break;
      }
         if  (linesensedr){
        turnRobot (-127,127);
        robotstate=TURNLEFT;
        break;
      }
         if (bumpsensed) {
        moveRobot(0);
        robotstate = STOPPED;
        break;
      }

     case TURNLEFT:
         if (linesensedm){
         moveRobot (127);
         robotstate=FORWARD;
         break;
    }

       if (linesensedl){
         turnRobot (127,-127);
         robotstate= TURNRIGHT;
         break;
       }
         if (bumpsensed) {
        moveRobot(0);
        robotstate = STOPPED;
        break;
  }
}
}
}
